/*
 * Copyright (c) 2020 Huawei Device Co., Ltd.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "hcsr04_control.h"
#include <stdio.h>
#include <sys/time.h>
#include "gpio_if.h"

#define GPIO_INPUT_PA19 19 // echo
#define GPIO_OUTPUT_PA20 20 // trig
#define DISTANCE_NUMBER 16
#define MEASURE_INTERVAL_MS 5
#define CONFIG_DELAY 20
#define CONFIG_SPEED (0.00034 / 2)
#define CONFIG_MI (1000000) // 微妙

static int g_init_height = 0;


void Hcs04InitRegisterIO(void)
{
    if (0 == GpioSetDir(GPIO_OUTPUT_PA20, GPIO_DIR_OUT)) {
        printf("set gpio success!PA20\n");
    }
    if (0 == GpioSetDir(GPIO_INPUT_PA19, GPIO_DIR_IN)) {
        printf("set gpio success!PA19\n");
    }
}


void Hcsr04SetInitHeight(int height)
{
    g_init_height = height;
}


static unsigned long GetMicrosecond(void)
{
    struct timeval tv;
    gettimeofday(&tv, NULL);

    return tv.tv_sec * CONFIG_MI + tv.tv_usec;
}

/**
 * @brief read hcsr04 module echo value and calculate distance
 * @return a distance by single measurement
*/
static float GetDistance(void)
{
    float distance;
    unsigned long  t;
    unsigned int flag = 0;
    enum GpioValue value = GPIO_VAL_LOW;
    GpioWrite(GPIO_OUTPUT_PA20, GPIO_VAL_HIGH);
    HAL_UDelay(CONFIG_DELAY);
    GpioWrite(GPIO_OUTPUT_PA20, GPIO_VAL_LOW);
    while (1) {
        GpioRead(GPIO_INPUT_PA19, &value);
        if (value == GPIO_VAL_HIGH && flag == 0) {
            t = GetMicrosecond();
            flag = 1;
        }
        if (value == GPIO_VAL_LOW && flag == 1) {
            t = GetMicrosecond() - t;
            break;
        }
    }

    distance = ((unsigned int)t) * CONFIG_SPEED;
    return distance;
}

/**
 * @brief  Bubble Sort,which is from small to large
*/
static void sort(float *buf, unsigned int length)
{
    int i, j;
    for (i = 0; i < length; i++) {
        for (j = 0; j < length - i - 1; j++) {
            if (buf[j] > buf[j+1]) {
                float tmp = buf[j];
                buf[j] = buf[j+1];
                buf[j+1] = tmp;
            }
        }
    }
}

/**
 * @brief median filter
 * @return distance
*/
static float FilteringData(void)
{
    float distBuff[DISTANCE_NUMBER];
    int total = DISTANCE_NUMBER;
    int i;
    for (i = 0; i < total; i++) {
        distBuff[i] = GetDistance();
        OS_MSleep(MEASURE_INTERVAL_MS);
    }
    sort(distBuff, total);
    return distBuff[(total - 1) / 2];
}


/**
 * @brief 
 * @return
*/
int  GetMeasurementDistance(float *value)
{
    int res = g_init_height - FilteringData() * 100;
    if (res < 0) {
        res = 0 - res;
    }
    *value = (float)(res * 1.0 / 100.0);
    return 1;
}
